/* Waypoint */

#include "Defines.h"
#include "Waypoint.h"
#include "GPS.h"

/* Fonction de calcul de cap à suivre pour rejoindre un point GPS */
float getBearing(float latPlaneur, float longPlaneur, float latWaypoint, float longWaypoint)
{
  float latPos, longPos, latWay, longWay;
  float bearing = 0;
  
  // Conversion en radians des coordonnées géographiques
  latPos = radians(latPlaneur);
  longPos = radians(longPlaneur);
  latWay = radians(latWaypoint);
  longWay = radians(longWaypoint);
  
  bearing = atan2((sin(longWay - longPos)*cos(latWay)),(cos(latPos)*sin(latWay) - sin(latPos)*cos(latWay)*cos(longWay - longPos)));
  
  bearing = degrees(bearing);
  
  if (bearing < 0)
  {
    bearing = 360 + bearing;
  }
  
  return bearing;
}

/* Fonction calculant la distance entre deux points GPS, la valeur retournée est en km */
float distance(float latPlaneur, float longPlaneur, float latWaypoint, float longWaypoint)
{
  double latPos, longPos, latWay, longWay, delta, x, y;
  double dist = 0;
  
  // Conversion en radians des coordonnées géographiques
  latPos = radians(latPlaneur);
  longPos = radians(longPlaneur);
  latWay = radians(latWaypoint);
  longWay = radians(longWaypoint);
  
  delta = longPos - longWay;
  
  dist = 2*asin(sqrt(pow(sin((latPos - latWay)/2),2) + cos(latPos)*cos(latWay)*pow(sin(delta/2),2)));

  dist = RAYON*dist;
  dist = (float) dist;
  
  return dist;
}

